package util;

/**
 *
 * @author DerrickNB
 */
public class AimingCalculator {     //used for making calculations for frisbee's flight path
    
    private DoubleArray1d altitudeRows;
    private DoubleArray1d altitudeColumns;
    private DoubleArray2d altitudeValues;
    
    private DoubleArray1d altitudeFlippedRows;
    private DoubleArray1d altitudeFlippedColumns;
    private DoubleArray2d altitudeFlippedValues;
    
    private DoubleArray1d azimuthRows;
    private DoubleArray1d azimuthColumns;
    private DoubleArray2d azimuthValues;
    
    private DoubleArray1d azimuthFlippedRows;
    private DoubleArray1d azimuthFlippedColumns;
    private DoubleArray2d azimuthFlippedValues;
    
    public double interpolate(double x, double x1, double x2, double y1, double y2){    //given line segment (x1, y1) (x2, y2) gives another (x,y) value
        
        //given two points on a line (x1,y1) and (x2,y2)
        //
        double y= (y1*(x2-x)+y2*(x-x1))/(x2-x1);
        return y;
    }
    
    public void loadCalibrationFiles (String folder){   //creates a reader so you can reac CSV files
        String filename;
        CSVFileReader reader = new CSVFileReader();
        
        filename = "altitude.csv";
        filename = folder+ "/" + filename;
        reader.ReadFile(filename);
        altitudeRows = reader.getRows();
        altitudeColumns = reader.getColumns();
        altitudeValues = reader.getValues();
        
        filename = "altitudeFlipped.csv";
        filename = folder+ "/" + filename;
        reader.ReadFile(filename);
        altitudeFlippedRows = reader.getRows();
        altitudeFlippedColumns = reader.getColumns();
        altitudeFlippedValues = reader.getValues();
        
        filename = "azimuth.csv";
        filename = folder+ "/" + filename;
        reader.ReadFile(filename);
        azimuthRows = reader.getRows();
        azimuthColumns = reader.getColumns();
        azimuthValues = reader.getValues();
        
        filename = "azimuthFlipped.csv";
        filename = folder+ "/" + filename;
        reader.ReadFile(filename);
        azimuthFlippedRows = reader.getRows();
        azimuthFlippedColumns = reader.getColumns();
        azimuthFlippedValues = reader.getValues();
        
        // System.out.println("Distances: "+altitudeRows);
        // System.out.println("==================");
        // System.out.println("Elevation: "+altitudeColumns);
        // System.out.println("==================");
        // System.out.println("Values: "+altitudeValues);
        // System.out.println("==================");
    }
    
    public double getShooterElevation(double targetDistance, double targetAltitude,boolean frisbeeFlipped){
        //Given a target Distance, Elevation and frisbee orientation,
        //to Caculate Azimuth and Elevation
        
        DoubleArray1d distance;
        DoubleArray1d elevation;
        DoubleArray2d altitude;
        
        if(frisbeeFlipped){
            distance = this.altitudeFlippedRows;
            elevation = this.altitudeFlippedColumns;
            altitude = this.altitudeFlippedValues;
        }
        else{
            distance = this.altitudeRows;
            elevation = this.altitudeColumns;
            altitude = this.altitudeValues;
        }
        
        int distanceIndex = -1;
        for(int i= 0; i < distance.size()-1; i++){
        if(targetDistance >= distance.get(i) && targetDistance <= distance.get(i+1)){
            distanceIndex=i;
            break;// if variable is true send answer to return function
        }
    }
        if(distanceIndex == -1){
            //didnt find value you were looking for
            return Double.NaN;
        }
        // System.out.println("Calculating Interpolation: ");
        // System.out.println("Stopped at Distance Index: "+distanceIndex);
        
        double d1= distance.get(distanceIndex);
        double d2 = distance.get(distanceIndex+1);
        // System.out.println("Estimated Distance of Unknown Value is: ("+ d1 +" and "+d2+ " meters)");
        
        //Discovered Values now Interpolating
        
        DoubleArray1d interpolatedAltitudes = new DoubleArray1d();
        
        for(int i=0; i<elevation.size(); i++){
            double a1= altitude.get(distanceIndex,i);
            double a2= altitude.get(distanceIndex+1,i);
            double altitude2 = interpolate(targetDistance, d1, d2,a1,a2);
            interpolatedAltitudes.add(altitude2);
        }
        // System.out.println("Interpolated Altitude: ");
        // System.out.println(interpolatedAltitudes);
        
        int altitudeIndex = -1;
        for(int i= 0; i< interpolatedAltitudes.size()-1; i++){
            if(targetAltitude >= interpolatedAltitudes.get(i) && targetAltitude <= interpolatedAltitudes.get(i+1)){
                altitudeIndex =i;
                break;
            }
        }
        if(altitudeIndex ==-1){
            return Double.NaN;
        }
        double a1= interpolatedAltitudes.get(altitudeIndex);
        double a2= interpolatedAltitudes.get(altitudeIndex+1);
        double e1 = elevation.get(altitudeIndex);
        double e2 = elevation.get(altitudeIndex+1);
        
        double elevation2 = interpolate(targetAltitude, a1,a2,e1,e2);
        
        return elevation2;
   
    }    
    
    public double getFrisbeeAzimuth(double targetDistance, double shooterElevation, boolean frisbeeFlipped){    
        // calculates the side to side movements of the frisbee 
        DoubleArray1d distance;
        DoubleArray1d elevation;
        DoubleArray2d azimuth;
        
        if(frisbeeFlipped){
            distance = this.azimuthFlippedRows;
            elevation = this.azimuthFlippedColumns;
            azimuth = this.azimuthFlippedValues;
        }
        else{
            distance = this.azimuthRows;
            elevation = this.azimuthColumns;
            azimuth = this.azimuthValues;
        }
        int distanceIndex = -1;
        for(int i= 0; i<distance.size()-1; i++){
            if(targetDistance >= distance.get(i) && targetDistance <= distance.get(i+1)){
                distanceIndex  = i;
                break;
            }
        }
        int elevationIndex= -1;
        for(int i= 0; i<elevation.size()-1; i++){
            if(shooterElevation >= elevation.get(i) && shooterElevation <= elevation.get(i+1)){
                elevationIndex  = i;
                break;
            }
        }
     double azimuth2;
     if(distanceIndex == -1 || elevationIndex == -1){
         azimuth2 = Double.NaN;
     }
     else{
         double d1 = distance.get(distanceIndex);
         double d2 = distance.get(distanceIndex+1);
         double azimuth_d1e1 = azimuth.get(distanceIndex, elevationIndex);
         double azimuth_d2e1 = azimuth.get(distanceIndex+1, elevationIndex);
         double a1 = interpolate(targetDistance, d1,d2,azimuth_d1e1,azimuth_d2e1);
         
         double azimuth_d1e2 = azimuth.get(distanceIndex, elevationIndex+1);
         double azimuth_d2e2 = azimuth.get(distanceIndex+1, elevationIndex+1);
         double a2 = interpolate(targetDistance, d1,d2,azimuth_d1e2,azimuth_d2e2);
         
         double e1= elevation.get(elevationIndex);
         double e2= elevation.get(elevationIndex+1);
         azimuth2= interpolate(shooterElevation, e1,e2,a1,a2);
         
         // System.out.println("-----");
         // System.out.println("Interpolated values (Approximated values): ");
         // System.out.println("Stopped at Azimuth Distance Index: "+ a1); 
         // System.out.println("Stopped at Azimuth Elevation Index: "+ a2);
         // System.out.println("-----");
         

        
     }
     return azimuth2;
   }
     public double getFrisbeeAltitude(double targetDistance, double shooterElevation, boolean frisbeeFlipped){
         //given the elevation of the shooter will find the altitude of the frisbee
        DoubleArray1d distance;
        DoubleArray1d elevation;
        DoubleArray2d altitude;
        
        if(frisbeeFlipped){
            distance = this.altitudeFlippedRows;
            elevation = this.altitudeFlippedColumns;
            altitude = this.altitudeFlippedValues;
        }
        else{
            distance = this.altitudeRows;
            elevation = this.altitudeColumns;
            altitude = this.altitudeValues;
        }
        int distanceIndex = -1;
        for(int i= 0; i<distance.size()-1; i++){
            if(targetDistance >= distance.get(i) && targetDistance <= distance.get(i+1)){
                distanceIndex  = i;
                break;
            }
        }
        int elevationIndex= -1;
        for(int i= 0; i<elevation.size()-1; i++){
            if(shooterElevation >= elevation.get(i) && shooterElevation <= elevation.get(i+1)){
                elevationIndex  = i;
                break;
            }
        }
     double altitude2;
     if(distanceIndex == -1 || elevationIndex == -1){
         altitude2 = Double.NaN;
     }
     else{
         double d1 = distance.get(distanceIndex);
         double d2 = distance.get(distanceIndex+1);
         double altitude_d1e1 = altitude.get(distanceIndex, elevationIndex);
         double altitude_d2e1 = altitude.get(distanceIndex+1, elevationIndex);
         double a1 = interpolate(targetDistance, d1,d2,altitude_d1e1,altitude_d2e1);
         
         double altitude_d1e2 = altitude.get(distanceIndex, elevationIndex+1);
         double altitude_d2e2 = altitude.get(distanceIndex+1, elevationIndex+1);
         double a2 = interpolate(targetDistance, d1,d2,altitude_d1e2,altitude_d2e2);
         
         double e1= elevation.get(elevationIndex);
         double e2= elevation.get(elevationIndex+1);
         altitude2= interpolate(shooterElevation, e1,e2,a1,a2);
         
         // System.out.println("Interpolated values (Approximated values): ");
         // System.out.println("Stopped at Altitude Distance Index: "+ a1); 
         // System.out.println("Stopped at Altitude Elevation Index: "+ a2);
         // System.out.println("==================");
         

        
     }
     return altitude2;
   }
}
